
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_approach.h"
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h"
#include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h"
#include "cyber/common/log.h"
#include "modules/planning/planning_base/common/frame.h"
#include "modules/planning/planning_base/common/planning_context.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::hdmap::PathOverlap;
using apollo::perception::TrafficLight;

StageResult TrafficLightLeftTurnWaitingZoneStageApproach::Process(
        const TrajectoryPoint& planning_init_point,
        Frame* frame) {
    ADEBUG << "stage: Approach - Processing left turn waiting zone approach stage";
    CHECK_NOTNULL(frame);
    CHECK_NOTNULL(context_);

    auto* context = GetContextAs<TrafficLightLeftTurnWaitingZoneContext>();
    const auto& scenario_config = context->scenario_config;
    auto& reference_line_info = frame->mutable_reference_line_info()->front();
    reference_line_info.LimitCruiseSpeed(scenario_config.approach_speed());
    
    StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    if (result.HasError()) {
        AERROR << "TrafficLightLeftTurnWaitingZoneStageApproach planning error";
    }
    
    // 检查必要的交通信号灯ID是否存在
    if (context->left_turn_traffic_light_id.empty() || context->forward_traffic_light_id.empty()) {
        AERROR << "Missing traffic light IDs, finishing scenario";
        return FinishScenario();
    }
    
    hdmap::PathOverlap forward_traffic_light_overlap;
    hdmap::PathOverlap left_turn_traffic_light_overlap_first;
    hdmap::PathOverlap left_turn_traffic_light_overlap_second;
    const std::vector<hdmap::PathOverlap> traffic_light_overlaps
            = reference_line_info.reference_line().map_path().signal_overlaps();
    
    // 查找对应的交通信号灯overlap
    for (const auto& overlap : traffic_light_overlaps) {
        if (overlap.object_id == context->forward_traffic_light_id) {
            forward_traffic_light_overlap = overlap;
        } else if (overlap.object_id == context->left_turn_traffic_light_id) {
            if (left_turn_traffic_light_overlap_second.start_s < overlap.start_s) {
                std::swap(left_turn_traffic_light_overlap_first, left_turn_traffic_light_overlap_second);
                left_turn_traffic_light_overlap_second = overlap;
            } else {
                left_turn_traffic_light_overlap_first = overlap;
            }
        }
    }
    
    const double adc_back_edge_s = reference_line_info.AdcSlBoundary().end_s();
    bool is_passed_forward_stop_line = adc_back_edge_s > forward_traffic_light_overlap.end_s;
    
    auto forward_signal_color = frame->GetSignal(context->forward_traffic_light_id).color();
    auto left_signal_color = frame->GetSignal(context->left_turn_traffic_light_id).color();
    
    AINFO << "Traffic light status - Forward: " << forward_signal_color 
          << ", Left turn: " << left_signal_color 
          << ", Passed forward stop line: " << is_passed_forward_stop_line;
    
    auto done_traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
    done_traffic_light->mutable_done_traffic_light_overlap_id()->Clear();
    
    // 阶段转换逻辑优化
    if (left_signal_color == TrafficLight::GREEN) {
        // 左转绿灯：可以直接通过路口
        if (is_passed_forward_stop_line) {
            AINFO << "Left turn green, passed forward stop line -> transitioning to CRUISE stage";
            return FinishStage("TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE");
        }
        done_traffic_light->add_done_traffic_light_overlap_id(forward_traffic_light_overlap.object_id);
    } else if (forward_signal_color == TrafficLight::GREEN) {
        // 直行绿灯：可以进入待转区
        done_traffic_light->add_done_traffic_light_overlap_id(left_turn_traffic_light_overlap_first.object_id);
        
        // 如果已经过了直行停止线，进入待转区
        if (is_passed_forward_stop_line) {
            AINFO << "Forward green, passed forward stop line -> transitioning to CREEP stage";
            return FinishStage("TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CREEP");
        }
    }
    
    // 如果已经过了直行停止线，无论信号灯状态如何，都应该进入待转区
    if (is_passed_forward_stop_line) {
        AINFO << "Passed forward stop line -> transitioning to CREEP stage";
        return FinishStage("TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CREEP");
    }
    
    AINFO << "Continuing in APPROACH stage, waiting for stop line crossing";
    return result.SetStageStatus(StageStatusType::RUNNING);
}

StageResult TrafficLightLeftTurnWaitingZoneStageApproach::FinishStage(std::string next_stage) {
    AINFO << "Finishing APPROACH stage, next stage: " << next_stage;
    injector_->planning_context()
            ->mutable_planning_status()
            ->mutable_traffic_light()
            ->mutable_done_traffic_light_overlap_id()
            ->Clear();
    next_stage_ = next_stage;
    return StageResult(StageStatusType::FINISHED);
}

}  // namespace planning
}  // namespace apollo
